#ifndef TOST_HPP
#define TOST_HPP

//    to check stm echo message, install gtkterm
//            sudo apt-get install gtkterm
//            set port and baudrate on menu item 'Configuration'-'Port'
//            port="/dev/ttyACM0"
//            baudrate=115200

#include <thread>
#include <chrono>

#define USBDUMP

class ToST
{
public:
   FILE *file;
public:
   ToST()
       : file(NULL)
   {

   }

   bool Initial(const char *port="/dev/ttyACM0")
   {
#ifdef USBDUMP
       file=fopen(port,"w");
       return file!=NULL;
#else
       return true;
#endif
   }

   void Rotate(int speed=20)
   {
       if(speed>0)
       {
           fprintf(file, "@mtr,%d,0!\n", speed);
           fflush(file);
       }
       else
       {
           fprintf(file, "@mtl,%d,0!\n", -speed);
           fflush(file);
       }
   }

   void MoveY(int speed=20)
   {
       if(speed>0)
       {
           fprintf(file, "@mff,%d,0!\n", speed);
           fflush(file);
       }
       else
       {
           fprintf(file, "@mbb,%d,180!\n", -speed);
           fflush(file);
       }
   }

   void MoveX(int speed=20)
   {
       if(speed>0)
       {
           fprintf(file, "@msl,%d,270!\n", speed);
           fflush(file);
       }
       else
       {
           fprintf(file, "@msr,%d,90!\n", -speed);
           fflush(file);
       }
   }

   void Stop()
   {
       fprintf(file, "@mss,0,0!\n");
       fflush(file);
   }

   void Shift(int angle, int speed=20)
   {
       fprintf(file, "@mfr,%d,%d!\n", speed, angle);
       fflush(file);
   }


   void Move(float speedXmmps=0, float speedYmmps=0, float speedRradps=0)
   {
       #ifdef USBDUMP
       fprintf(file, "%g,%g,%g\n", speedXmmps, speedYmmps, speedRradps);
       fflush(file);
#endif
   }

   void End()
   {
       #ifdef USBDUMP
       fclose(file);
#endif
   }

   void R90(bool left=true)
   {
       double rad=0.3;
       Move(0,0,left?rad:-rad);
       int ms=1000*CV_PI/rad*0.5;
       std::cout<<"\nR90="<<ms<<"\n"<<std::flush;
       std::this_thread::sleep_for(std::chrono::milliseconds(ms));
       Move();
   }

   void Unlock(float r)
   {
       Move(65535,65535,r);
   }

   void test(const char *port="/dev/ttyACM0")
   {
       if(Initial(port))
       {
           int waitms=200;

           int runms=1000;
//           MoveX(35);
//           std::this_thread::sleep_for(std::chrono::milliseconds(runms));
//           Rotate();
//           std::this_thread::sleep_for(std::chrono::milliseconds(waitms));

//           MoveY(35);
//           std::this_thread::sleep_for(std::chrono::milliseconds(runms));
//          Rotate();

//          std::this_thread::sleep_for(std::chrono::milliseconds(waitms));
//           MoveX(30);
//           std::this_thread::sleep_for(std::chrono::milliseconds(runms));
//           Rotate();
//           std::this_thread::sleep_for(std::chrono::milliseconds(waitms));
//           MoveY(-30);

//           Move(-0,20,-0.02);
//           std::this_thread::sleep_for(std::chrono::milliseconds(runms));

//           Rotate();
//           std::this_thread::sleep_for(std::chrono::milliseconds(waitms));
//           Stop();
//           Move();

           R90();
//           Unlock(0);
           std::this_thread::sleep_for(std::chrono::milliseconds(runms));
            R90(false);
//           Unlock(1);
           std::this_thread::sleep_for(std::chrono::milliseconds(waitms));

           End();
       }
   }

};

#endif // TOST_HPP

